// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; version 2 of the License.

// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.

#include "GravBody.h"

#include "hydroapp.h"
#include "physics.h"

#ifndef PI
#define PI 3.141592653589793238462643383279502884
#endif

GravBody::GravBody(std::string _name, Body* _parent, Orbit* _orbit, double _mass, double _radius, double _rotperiod, double _rotoffset, double _rotinclination, double _rotnode) : Body(_parent, _mass)
{
	name = _name;
	radius = _radius;
	orbit = _orbit;
	sphere = createSphere(name, 300 /*radius/31855.05*/, 64, 64);
	ent = app->mSceneMgr->createEntity(name, name);
	ent->setMaterialName(name);
//	SceneNode* rnode = tmp->node->createChildSceneNode();
	//mass = _mass;
	rotsn = node->createChildSceneNode();
	rotsn->attachObject(ent);
	//physBody = new PhysBody(_orbit, _mass, _radius);
	position = VECTOR(0,0,0);
	rposition = Ogre::Vector3(0,0,0);
	velocity = VECTOR(0,0,0);
	//node = sn;
	tracepoints = new std::vector<VECTOR>();
	orbit->sample(2454638.82294, orbit->getPeriod(), 200, tracepoints);
	orbtrace = app->mSceneMgr->createManualObject(name + "Trace");
	orbtrace->estimateVertexCount(200);
	orbtrace->estimateIndexCount(200);
	orbtrace->setDynamic(true);
	if (_orbit->getPeriod() != 0.0) {
		//VECTOR adjvec(0,0,0);
		if (parent != NULL) {
			parent->node->attachObject(orbtrace);
			//adjvec = _orbit->parent->physBody->orbit->computePosition(2454638.82294);
		} else {
			app->staticNode->attachObject(orbtrace);
		}

	orbtrace->begin("orbit", Ogre::RenderOperation::OT_LINE_STRIP);
	std::vector<VECTOR>::iterator iter;
	VECTOR tmpvec(0,0,0);
	for (iter = tracepoints->begin(); iter != tracepoints->end(); iter++) {
		tmpvec = (*iter) / 50000000.;
		orbtrace->position(tmpvec.x, tmpvec.y, tmpvec.z);
	}
	orbtrace->end();
	}
	rotperiod = _rotperiod;
	rotinclination = _rotinclination;
	rotnode = _rotnode;
	rotepoch = 2451545.0;
	rotoffset = _rotoffset;
	reforientation = (Quat::yrotation(-rotinclination) * Quat::zrotation(-rotnode));// * parent->reforientation;
}

GravBody::~GravBody(void)
{
	//OGRE_DELETE orbtrace;
	//OGRE_DELETE ent;
	//OGRE_DELETE rotsn;
	//OGRE_DELETE tracepoints;

	
	//delete node;
	//Body::~Body();
}

Vec3 estimate_velocity(GravBody* body) {
	double T = 2*PI * sqrt( pow(body->radius,3.0) / (G * ( body->mass )  ) );
	double dstep =  T / ( 3600.0 * 24. * 360. * 60. * 24);
	double dnow = app->clock->jd;
	Vec3 p1 = body->orbit->computePosition(dnow-dstep/2.);
	Vec3 p2 = body->orbit->computePosition(dnow+dstep/2.);
	return (p2 - p1)/(dstep*3600.0*24.0);
}

void GravBody::update()
{
//	WaitForSingleObject(physBody->phys_mutex, INFINITE);
//	ReleaseMutex(physBody->phys_mutex);
	position = orbit->computePosition(app->clock->jd);

	double rotations = (app->clock->jd - rotepoch) / rotperiod;
	double wholeRotations = floor(rotations);
	double remainder = rotations - wholeRotations;
	remainder += 0.5;
	spin = Quat::zrotation(remainder * 2 * PI - rotoffset);
	position = parent->reforientation.toMatrix3() * position;
	gposition = parent->gposition + position;
	//position += parent->position;
	rposition.x = (gposition.x) / 50000000.;
	rposition.y = (gposition.y) / 50000000.;
	rposition.z = (gposition.z) / 50000000.;
	node->setPosition(rposition);
	node->setOrientation(reforientation.w, reforientation.x, reforientation.y, reforientation.z);
	rotsn->setOrientation(spin.w, spin.x, spin.y, spin.z);
	velocity = estimate_velocity(this);
	Body::update();
//	velocity.x = tmpv.x;
//	velocity.y = tmpv.y;
//	velocity.z = tmpv.z;
	//if (node != NULL) {

}
/*
PhysBody::PhysBody(Orbit *_orbit, double _mass, double _radius)
{
	orbit = _orbit;
	mass = _mass;
	radius = _radius;
	position = VECTOR(0,0,0);
	velocity = VECTOR(0,0,0);
	phys_mutex = CreateMutex(NULL, FALSE, NULL);
}

PhysBody::~PhysBody() {
	CloseHandle(phys_mutex);
}

void PhysBody::update(double jd)
{
	position = orbit->computePosition(jd);
	WaitForSingleObject(phys_mutex, INFINITE);
	tmpposition.x = position.x;
	tmpposition.y = position.y;
	tmpposition.z = position.z;
	ReleaseMutex(phys_mutex);
}
*/